                                                                     
                                                                     
                                                                     
                                             
/*
* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

#ifndef TOM_TEST_H
#define TOM_TEST_H

class TomTest : public Test
{
public:
	b2Body* rFoot;
	b2Body* rCalf;
	b2Body* rThigh;
	b2Body* lFoot;
	b2Body* lCalf;
	b2Body* lThigh;
	b2RevoluteJoint* rAnkle;
	b2RevoluteJoint* rKnee;
	b2RevoluteJoint* hip;
	b2RevoluteJoint* lKnee;
	b2RevoluteJoint* lAnkle;

	TomTest()
	{
	    //minimal representation
		const float brx=0.0f;
		const float bry=0.0f;
		const float bq1=0.0f*b2_pi;//lFoot vs. ground
		const float bq2=0.5f*b2_pi;//lAnkle
		const float bq3=0.0f*b2_pi;//lKnee
		const float bq4=0.0f*b2_pi;//hip
		const float bq5=0.0f*b2_pi;//rKnee
		const float bq6=0.5f*b2_pi;//rAnkle





		const float footLength=0.7f;
		const float footWidth=0.1f;
		const float calfLength=1.5f;
		const float calfWidth=0.04f;
		const float thighLength=1.5f;
		const float thighWidth=0.7f;

		const float ankleOffset = 0.1f;
		const float ankleElevationAngle = atanf(footWidth/(footLength-ankleOffset));


		const float ankleLimL = 0.0f;
		const float ankleLimH = 0.1f;
		const float kneeLimL = -0.7f;
		const float kneeLimH = 0.0f;

		const float angularDamping = 1.0f;
		const float linearDamping = 0.2f;
		const float groundAngle = 0.2f;

		b2BodyDef bd;
		bd.angle = groundAngle;
		b2Body* ground = m_world->CreateBody(&bd);

		b2PolygonDef sd;
		sd.density = 0.0f;
		sd.restitution = 0.0f;
		sd.friction = 1.0f;


		sd.SetAsBox(10.0f, 0.1f, b2Vec2(0.0f, -0.1f), 0.0f * b2_pi);
		ground->CreateShape(&sd);



		b2BodyDef lfoot;
		lfoot.angularDamping = angularDamping;
		lfoot.linearDamping = linearDamping;

		b2RevoluteJointDef lAnkleDef;

		b2BodyDef lcalf;
		lcalf.angularDamping = angularDamping;
		lcalf.linearDamping = linearDamping;

		b2RevoluteJointDef lKneeDef;

		b2BodyDef lthigh;
		lthigh.angularDamping = angularDamping;
		lthigh.linearDamping = linearDamping;

		b2RevoluteJointDef hipDef;

		b2BodyDef rthigh;
		rthigh.angularDamping = angularDamping;
		rthigh.linearDamping = linearDamping;

		b2RevoluteJointDef rKneeDef;

		b2BodyDef rcalf;
		rcalf.angularDamping = angularDamping;
		rcalf.linearDamping = linearDamping;

		b2RevoluteJointDef rAnkleDef;

		b2BodyDef rfoot;
		rfoot.angularDamping = angularDamping;
		rfoot.linearDamping = linearDamping;


		b2PolygonDef thighShape;
		thighShape.SetAsBox(thighLength, thighWidth, b2Vec2(0.0f, 0.0f), 0.0f * b2_pi);
		thighShape.density = 1.0f;
		thighShape.friction = 1.0f;
		thighShape.filter.groupIndex = -1;

		b2PolygonDef calfShape;
		calfShape.SetAsBox(calfLength, calfWidth, b2Vec2(0.0f, 0.0f), 0.0f * b2_pi);
		calfShape.density = 1.0f;
		thighShape.friction = 1.0f;
		calfShape.filter.groupIndex = -1;

		b2PolygonDef footShape;
		footShape.SetAsBox(footLength, footWidth, b2Vec2(0.0f, footWidth), 0.0f);
		footShape.density = 1.0f;
		thighShape.friction = 1.0f;
		footShape.filter.groupIndex = -1;



		lfoot.position.Set(brx, bry);
		lfoot.angle = groundAngle+bq1;
		lFoot = m_world->CreateBody(&lfoot);
		lFoot->CreateShape(&footShape);


		lcalf.angle = lfoot.angle+bq2;
		lcalf.position.Set(lfoot.position.x+(footLength-ankleOffset)*cosf(lfoot.angle+ankleElevationAngle)+calfLength*cosf(lcalf.angle),lfoot.position.y+(footLength-ankleOffset)*sinf(lfoot.angle+ankleElevationAngle)+calfLength*sinf(lcalf.angle));
		lCalf = m_world->CreateBody(&lcalf);
		lCalf->CreateShape(&calfShape);


		lAnkleDef.Initialize(lFoot, lCalf, b2Vec2(lfoot.position.x+(footLength-ankleOffset)*cosf(lfoot.angle+ankleElevationAngle),lfoot.position.y+(footLength-ankleOffset)*sinf(lfoot.angle+ankleElevationAngle)));
		//lAnkleDef.referenceAngle=q2;
		lAnkleDef.lowerAngle=ankleLimL;
		lAnkleDef.upperAngle=ankleLimH;
		lAnkleDef.enableLimit=true;
		lAnkle = (b2RevoluteJoint*)m_world->CreateJoint(&lAnkleDef);
		


		lthigh.angle = lcalf.angle+bq3;
		lthigh.position.Set(lcalf.position.x+calfLength*cosf(lcalf.angle)+thighLength*cosf(lthigh.angle), lcalf.position.y+calfLength*sinf(lcalf.angle)+thighLength*sinf(lthigh.angle));
		lThigh = m_world->CreateBody(&lthigh);
		lThigh->CreateShape(&thighShape);


		lKneeDef.Initialize(lCalf, lThigh, b2Vec2(lcalf.position.x+calfLength*cosf(lcalf.angle),lcalf.position.y+calfLength*sinf(lcalf.angle)));
		//lKneeDef.referenceAngle=q3;
		lKneeDef.lowerAngle=kneeLimL;
		lKneeDef.upperAngle=kneeLimH;
		lKneeDef.enableLimit=true;
		lKnee = (b2RevoluteJoint*)m_world->CreateJoint(&lKneeDef);


		rthigh.angle = lthigh.angle-bq4;
		rthigh.position.Set(lthigh.position.x+thighLength*cosf(lthigh.angle)-thighLength*cosf(rthigh.angle), lthigh.position.y+thighLength*sinf(lthigh.angle)-thighLength*sinf(rthigh.angle));
		rThigh = m_world->CreateBody(&rthigh);
		rThigh->CreateShape(&thighShape);


		hipDef.Initialize(lThigh, rThigh, b2Vec2(lthigh.position.x+thighLength*cosf(lthigh.angle),lthigh.position.y+thighLength*sinf(lthigh.angle)));
		//hipDef.referenceAngle=q4;
		hip = (b2RevoluteJoint*)m_world->CreateJoint(&hipDef);


		rcalf.angle = rthigh.angle-bq5;
		rcalf.position.Set(rthigh.position.x-thighLength*cosf(rthigh.angle)-calfLength*cosf(rcalf.angle), rthigh.position.y-thighLength*sinf(rthigh.angle)-calfLength*sinf(rcalf.angle));
		rCalf = m_world->CreateBody(&rcalf);
		rCalf->CreateShape(&calfShape);


		rKneeDef.Initialize(rCalf, rThigh , b2Vec2(rthigh.position.x-thighLength*cosf(rthigh.angle),rthigh.position.y-thighLength*sinf(rthigh.angle)));
		//rKneeDef.referenceAngle=q5;
		rKneeDef.lowerAngle=kneeLimL;
		rKneeDef.upperAngle=kneeLimH;
		rKneeDef.enableLimit=true;
		rKnee = (b2RevoluteJoint*)m_world->CreateJoint(&rKneeDef);


		rfoot.angle = rcalf.angle-bq6;
		rfoot.position.Set(rcalf.position.x-calfLength*cosf(rcalf.angle)-(footLength-ankleOffset)*cosf(rfoot.angle+ankleElevationAngle),rcalf.position.y-calfLength*sinf(rcalf.angle)-(footLength-ankleOffset)*sinf(rfoot.angle+ankleElevationAngle));
		rFoot = m_world->CreateBody(&rfoot);
		rFoot->CreateShape(&footShape);


		rAnkleDef.Initialize(rFoot, rCalf, b2Vec2(rcalf.position.x-calfLength*cosf(rcalf.angle),rcalf.position.y-calfLength*sinf(rcalf.angle)));
		//rAnkleDef.referenceAngle=q6;
		rAnkleDef.lowerAngle=ankleLimL;
		rAnkleDef.upperAngle=ankleLimH;
		rAnkleDef.enableLimit=true;
		rAnkle = (b2RevoluteJoint*)m_world->CreateJoint(&rAnkleDef);




		lFoot->SetMassFromShapes();
		lCalf->SetMassFromShapes();
		lThigh->SetMassFromShapes();
		rThigh->SetMassFromShapes();
		rCalf->SetMassFromShapes();
		rFoot->SetMassFromShapes();
		//lAnkle->EnableMotor(false);
		//lKnee->EnableMotor(false);









		const float rx=0.0f;
		const float ry=2.0f;
		const float q1=bq1-0.0f;//lFoot vs. ground
		const float q2=bq2+0.0f;//lAnkle
		const float q3=bq3-0.0f;//lKnee
		const float q4=bq4+0.0f;//hip
		const float q5=bq5-0.0f;//rKnee
		const float q6=bq6+0.0f;//rAnkle
		const float csq1=q1+groundAngle;
		const float csq2=csq1+q2;
		const float csq3=csq2+q3;
		const float csq4=csq3-q4;
		const float csq5=csq4-q5;
		const float csq6=csq5-q6;
		const float rdx=0.0f;
		const float rdy=0.0f;
		const float qd1=-5.0f;
		const float qd2=5.0f;
		const float qd3=0.0f;
		const float qd4=0.0f;
		const float qd5=0.0f;
		const float qd6=5.0f;
		const float csqd1=qd1;
		const float csqd2=csqd1+qd2;
		const float csqd3=csqd2+qd3;
		const float csqd4=csqd3-qd4;
		const float csqd5=csqd4-qd5;
		const float csqd6=csqd5-qd6;


		lFoot->SetXForm(b2Vec2(rx, ry),csq1);
		lFoot->SetLinearVelocity(b2Vec2(rdx, rdy));
		lFoot->SetAngularVelocity(csqd1);

		lCalf->SetXForm(b2Vec2(lFoot->GetPosition().x+(footLength-ankleOffset)*cosf(csq1+ankleElevationAngle)+calfLength*cosf(csq2),lFoot->GetPosition().y+(footLength-ankleOffset)*sinf(csq1+ankleElevationAngle)+calfLength*sinf(csq2)),csq2);
		lCalf->SetAngularVelocity(csqd2);
		
		lThigh->SetXForm(b2Vec2(lCalf->GetPosition().x+calfLength*cosf(csq2)+thighLength*cosf(csq3), lCalf->GetPosition().y+calfLength*sinf(csq2)+thighLength*sinf(csq3)), csq3);
		lThigh->SetAngularVelocity(csqd3);

		//hip
		rThigh->SetXForm(b2Vec2(lThigh->GetPosition().x+thighLength*cosf(csq3)-thighLength*cosf(csq4),lThigh->GetPosition().y+thighLength*sinf(csq3)-thighLength*sinf(csq4)),csq4);
		rThigh->SetAngularVelocity(csqd4);

		rCalf->SetXForm(b2Vec2(rThigh->GetPosition().x-thighLength*cosf(csq4)-calfLength*cosf(csq5),rThigh->GetPosition().y-thighLength*sinf(csq4)-calfLength*sinf(csq5)),csq5);
		rCalf->SetAngularVelocity(csqd5);

		rFoot->SetXForm(b2Vec2(rCalf->GetPosition().x-calfLength*cosf(csq5)-(footLength-ankleOffset)*cosf(csq6+ankleElevationAngle),rCalf->GetPosition().y-calfLength*sinf(csq5)-(footLength-ankleOffset)*sinf(csq6+ankleElevationAngle)),csq6);
		rFoot->SetAngularVelocity(csqd6);

	}

	static Test* Create()
	{
		return new TomTest;
	}

};

#endif
